package com.yukefms.rule.formula.basis;

import com.yukefms.engine.running.MatchedStatus;
import com.yukefms.engine.running.RunningContext;
import com.yukefms.jointpoint.BodyPointCloud;
import com.yukefms.jointpoint.GeometryAdaptorForPointCalculator;
import com.yukefms.jointpoint.TolerantEpsilon;
import com.yukefms.jointpoint.interfaces.Position;
import com.yukefms.rule.formula.interfaces.AbstractStaticBAFormula;
import com.yukefms.rule.formula.interfaces.StaticBAFormula;
import third_party.ihmc.euclid.tools.EuclidCoreTools;

/**
 * @Description
 * @Author Ping
 * @Date 2021/6/29 23:51
 **/
public class In2DParallelFormula extends AbstractStaticBAFormula implements StaticBAFormula {
    private String line1Start ;
    private String line1End ;
    private String line2Start ;
    private String line2End ;

    public In2DParallelFormula(String line1Start, String line1End, String line2Start, String line2End) {
        super(line1Start, line1End, line2Start, line2End);
        this.line1Start = line1Start;
        this.line1End = line1End;
        this.line2Start = line2Start;
        this.line2End = line2End;
    }

    @Override
    public MatchedStatus matched(BodyPointCloud bodyPointCloud, RunningContext context) {

        if (GeometryAdaptorForPointCalculator.isTwoLine2DParallel(bodyPointCloud.getPosition(line1Start),
                                                                      bodyPointCloud.getPosition(line1End),
                                                                      bodyPointCloud.getPosition(line2Start),
                                                                      bodyPointCloud.getPosition(line2End),
                                                                       TolerantEpsilon.ANGLE_EPSILON)) {
            // if has successor , then pass to the next formula.
            if (this.successor != null) {
                return this.successor.matched(bodyPointCloud, context);
            } else {
                return MatchedStatus.TRUE;
            }

        }
        // does not matched.
        return MatchedStatus.FALSE;
    }

    private double realAngleComparing(Position line1Start, Position line1End, Position line2Start, Position line2End) {
        double firstVectorX = line1End.getX() - line1Start.getX() ;
        double firstVectorY = line1End.getY() - line1Start.getY() ;
        double firstVectorZ = line1End.getZ() - line1Start.getZ() ;
        double secondVectorX = line2End.getX() - line2Start.getX() ;
        double secondVectorY = line2End.getY() - line2Start.getY() ;
        double secondVectorZ = line2End.getZ() - line2Start.getZ() ;

        double firstVectorLength = EuclidCoreTools.norm(firstVectorX, firstVectorY, firstVectorZ);
        double secondVectorLength = EuclidCoreTools.norm(secondVectorX, secondVectorY, secondVectorZ);

        double dot = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ;
       // return Math.abs(dot / (firstVectorLength * secondVectorLength)) > EuclidCoreTools.cos(TolerantEpsilon.ANGLE_EPSILON);

        return Math.abs(dot / (firstVectorLength * secondVectorLength)) - EuclidCoreTools.cos(TolerantEpsilon.ANGLE_EPSILON);

    }
}
